#define _USE_MATH_DEFINES
#include <math.h>
#include <stdio.h>
#include <ctype.h>
#include <string.h>
#include <algorithm>
#include "Recast.h"
#include "InputGeom.h"
#include "ChunkyTriMesh.h"
#include "MeshLoader.h"
#include "DetourNavMesh.h"
#include "Finalizer.h"
#include "DetourAlloc.h"
#include "fixed_lookuptbl.h"
#include "fixed64Helper.h"

static bool intersectSegmentTriangle(const fixed64* sp, const fixed64* sq,
									 const fixed64* a, const fixed64* b, const fixed64* c,
	fixed64&t)
{
	fixed64 v, w;
	fixed64 ab[3], ac[3], qp[3], ap[3], norm[3], e[3];
	rcVsub(ab, b, a);
	rcVsub(ac, c, a);
	rcVsub(qp, sp, sq);
	
	// Compute triangle normal. Can be precalculated or cached if
	// intersecting multiple segments against the same triangle
	rcVcross(norm, ab, ac);
	
	// Compute denominator d. If d <= 0, segment is parallel to or points
	// away from triangle, so exit early
	fixed64 d = rcVdot(qp, norm);
	if (d <= fixed64::ZERO) 
		return false;
	
	// Compute intersection t value of pq with plane of triangle. A ray
	// intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay
	// dividing by d until intersection has been found to pierce triangle
	rcVsub(ap, sp, a);
	t = rcVdot(ap, norm);
	if (t < fixed64::ZERO) return false;
	if (t > d) return false; // For segment; exclude this code line for a ray test
	
	// Compute barycentric coordinate components and test if within bounds
	rcVcross(e, qp, ap);
	v = rcVdot(ac, e);
	if (v < fixed64::ZERO || v > d) return false;
	w = -rcVdot(ab, e);
	if (w < fixed64::ZERO || v + w > d) return false;
	
	// Segment/ray intersects triangle. Perform delayed division
	t /= d;
	
	return true;
}

InputGeom::InputGeom() :
	m_chunkyMesh(0),
	m_mesh(0),
	m_hasBuildSettings(false),
	m_offMeshConCount(0),
	m_volumeCount(0)
{
}

InputGeom::~InputGeom()
{
	delete m_chunkyMesh;
	delete m_mesh;
}
		
bool InputGeom::loadMesh(const std::string& filepath)
{
	if (m_chunkyMesh)
	{
		delete m_chunkyMesh;
		m_chunkyMesh = 0;
	}
	if (m_mesh)
	{
		delete m_mesh;
		m_mesh = 0;
	}
	m_offMeshConCount = 0;
	m_volumeCount = 0;
	
	m_mesh = new rcMeshLoaderObj;
	if (!m_mesh)
	{
		//ctx->log(RC_LOG_ERROR, "loadMesh: Out of memory 'm_mesh'.");
		return false;
	}

	if (!m_mesh->load(filepath))
	{
		//ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load '%s'", filepath.c_str());
		return false;
	}

	rcCalcBounds(m_mesh->getVerts(), m_mesh->getVertCount(), m_meshBMin, m_meshBMax);

	m_chunkyMesh = new rcChunkyTriMesh;
	if (!m_chunkyMesh)
	{
		//ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Out of memory 'm_chunkyMesh'.");
		return false;
	}

	if (!rcCreateChunkyTriMesh(m_mesh->getVerts(), m_mesh->getTris(), m_mesh->getTriCount(), 256, m_chunkyMesh))
	{
		//ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Failed to build chunky mesh.");
		return false;
	}

	return true;
}

bool InputGeom::loadMesh(unsigned char* buf, int length)
{
	if (m_chunkyMesh)
	{
		delete m_chunkyMesh;
		m_chunkyMesh = 0;
	}
	if (m_mesh)
	{
		delete m_mesh;
		m_mesh = 0;
	}
	m_offMeshConCount = 0;
	m_volumeCount = 0;
	if (!buf || length <= 0)
		return false;

	m_mesh = new rcMeshLoaderObj;
	if (!m_mesh)
	{
		//ctx->log(RC_LOG_ERROR, "loadMesh: Out of memory 'm_mesh'.");
		return false;
	}

	if (!m_mesh->load(buf,length))
	{
		//ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load '%s'", filepath.c_str());
		return false;
	}

	rcCalcBounds(m_mesh->getVerts(), m_mesh->getVertCount(), m_meshBMin, m_meshBMax);

	m_chunkyMesh = new rcChunkyTriMesh;
	if (!m_chunkyMesh)
	{
		//ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Out of memory 'm_chunkyMesh'.");
		return false;
	}

	if (!rcCreateChunkyTriMesh(m_mesh->getVerts(), m_mesh->getTris(), m_mesh->getTriCount(), 256, m_chunkyMesh))
	{
		//ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Failed to build chunky mesh.");
		return false;
	}

	return true;
}

bool InputGeom::loadGeomSet(const std::string& filepath)
{
	FILE* fp = fopen(filepath.c_str(), "rb");
	if (!fp)
	{
		return false;
	}
	Finalizer tf([&]()->void {fclose(fp); });

	if (fseek(fp, 0, SEEK_END) != 0)
	{
		return false;
	}

	long length = ftell(fp);
	if (length <= 0)
	{
		return false;
	}

	if (fseek(fp, 0, SEEK_SET) != 0)
	{
		return false;
	}

	fixedByte* buf = (fixedByte*)dtAlloc(length, DT_ALLOC_TEMP);
	if (!buf)
	{
		return false;
	}
	Finalizer fb([&]()->void { dtFree(buf); });

	size_t readLen = fread(buf, length, 1, fp);
	if (readLen != 1)
	{
		return false;
	}

	return loadGeomSet(buf, length);
}

bool InputGeom::loadGeomSet(unsigned char* buf, int length)
{
	if (!buf || length <= 0)
		return false;
	int pos = 0;
	fixedByte endianValue = buf[pos++];
	bool swapEndian = endianValue != getEndianValue();
	BuildSettings* settings = &m_buildSettings;

	if (!read_fixed64(buf, length, pos, settings->cellSize, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, settings->cellHeight, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, settings->agentHeight, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, settings->agentRadius, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, settings->agentMaxClimb, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, settings->agentMaxSlope, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, settings->regionMinSize, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, settings->regionMergeSize, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, settings->edgeMaxLen, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, settings->edgeMaxError, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, settings->vertsPerPoly, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, settings->detailSampleDist, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, settings->detailSampleMaxError, swapEndian))
		return false;
	if (!read_fixedvalue_for_int(buf, length, pos, settings->partitionType, swapEndian))
		return false;
	if (!read_fixed64_v3(buf, length, pos, settings->navMeshBMin, swapEndian))
		return false;
	if (!read_fixed64_v3(buf, length, pos, settings->navMeshBMax, swapEndian))
		return false;
	if (!read_fixed64(buf, length, pos, settings->tileSize, swapEndian))
		return false;
	// Store off-mesh links.
	if (!read_fixedvalue_for_int(buf, length, pos, m_offMeshConCount, swapEndian))
		return false;

	for (int i = 0; i < m_offMeshConCount; ++i)
	{
		fixed64* v = &m_offMeshConVerts[i * 3 * 2];
		if (!read_fixed64_v3(buf, length, pos, v, swapEndian))
			return false;
		if (!read_fixed64_v3(buf, length, pos, v + 3, swapEndian))
			return false;
		if (!read_fixed64(buf, length, pos, m_offMeshConRads[i], swapEndian))
			return false;
		int flag = m_offMeshConDirs[i] | (m_offMeshConAreas[i] << 8) | (m_offMeshConFlags[i] << 16);
		if (!read_fixedvalue_for_int(buf, length, pos, flag, swapEndian))
			return false;
		m_offMeshConDirs[i] = (unsigned char)(flag & 0xFF);
		m_offMeshConAreas[i] = (unsigned char)((flag >> 8) & 0xFF);
		m_offMeshConFlags[i] = (unsigned short)((flag >> 16) & 0xFFFF);
	}

	// Convex volumes
	if (!read_fixedvalue_for_int(buf, length, pos, m_volumeCount, swapEndian))
		return false;

	for (int i = 0; i < m_volumeCount; ++i)
	{
		ConvexVolume* vol = &m_volumes[i];
		if (!read_fixedvalue_for_int(buf, length, pos, vol->nverts, swapEndian))
			return false;
		if (!read_fixedvalue_for_int(buf, length, pos, vol->area, swapEndian))
			return false;
		if (!read_fixed64(buf, length, pos, vol->hmin, swapEndian))
			return false;
		if (!read_fixed64(buf, length, pos, vol->hmax, swapEndian))
			return false;
		//fprintf(fp, "v %d %d %f %f\n", vol->nverts, vol->area, vol->hmin, vol->hmax);
		for (int j = 0; j < vol->nverts; ++j)
		{
			if (!read_fixed64_v3(buf, length, pos, &vol->verts[j * 3], swapEndian))
				return false;
		}
	}

	if (pos != length)
		return false;
	m_hasBuildSettings = true;
	return true;
}

static bool isectSegAABB(const fixed64* sp, const fixed64* sq,
	const fixed64* amin, const fixed64* amax,
	fixed64& tmin, fixed64& tmax)
{
	static const fixed64 EPS = fixed64::Epsilon;

	fixed64 d[3];
	d[0] = sq[0] - sp[0];
	d[1] = sq[1] - sp[1];
	d[2] = sq[2] - sp[2];
	tmin = fixed64::_0;
	tmax = fixed64::_1;

	for (int i = 0; i < 3; i++)
	{
		if (fixed64::abs(d[i]) < EPS)
		{
			if (sp[i] < amin[i] || sp[i] > amax[i])
				return false;
		}
		else
		{
			const fixed64 ood = fixed64::_1 / d[i];
			fixed64 t1 = (amin[i] - sp[i]) * ood;
			fixed64 t2 = (amax[i] - sp[i]) * ood;
			if (t1 > t2) { fixed64 tmp = t1; t1 = t2; t2 = tmp; }
			if (t1 > tmin) tmin = t1;
			if (t2 < tmax) tmax = t2;
			if (tmin > tmax) return false;
		}
	}

	return true;
}

bool InputGeom::raycastMesh(fixed64* src, fixed64* dst, fixed64& tmin)
{
	// Prune hit ray.
	fixed64 btmin, btmax;
	if (!isectSegAABB(src, dst, m_meshBMin, m_meshBMax, btmin, btmax))
		return false;
	fixed64 p[2], q[2];
	p[0] = src[0] + (dst[0]-src[0])*btmin;
	p[1] = src[2] + (dst[2]-src[2])*btmin;
	q[0] = src[0] + (dst[0]-src[0])*btmax;
	q[1] = src[2] + (dst[2]-src[2])*btmax;
	
	int cid[512];
	const int ncid = rcGetChunksOverlappingSegment(m_chunkyMesh, p, q, cid, 512);
	if (!ncid)
		return false;
	
	tmin = fixed64::_1;
	bool hit = false;
	const fixed64* verts = m_mesh->getVerts();
	
	for (int i = 0; i < ncid; ++i)
	{
		const rcChunkyTriMeshNode& node = m_chunkyMesh->nodes[cid[i]];
		const int* tris = &m_chunkyMesh->tris[node.i*3];
		const int ntris = node.n;

		for (int j = 0; j < ntris*3; j += 3)
		{
			fixed64 t = fixed64::_1;
			if (intersectSegmentTriangle(src, dst,
										 &verts[tris[j]*3],
										 &verts[tris[j+1]*3],
										 &verts[tris[j+2]*3], t))
			{
				if (t < tmin)
					tmin = t;
				hit = true;
			}
		}
	}
	
	return hit;
}

void InputGeom::addOffMeshConnection(const fixed64* spos, const fixed64* epos, const fixed64 rad,
									 unsigned char bidir, unsigned char area, unsigned short flags)
{
	if (m_offMeshConCount >= MAX_OFFMESH_CONNECTIONS) return;
	fixed64* v = &m_offMeshConVerts[m_offMeshConCount*3*2];
	m_offMeshConRads[m_offMeshConCount] = rad;
	m_offMeshConDirs[m_offMeshConCount] = bidir;
	m_offMeshConAreas[m_offMeshConCount] = area;
	m_offMeshConFlags[m_offMeshConCount] = flags;
	m_offMeshConId[m_offMeshConCount] = 1000 + m_offMeshConCount;
	rcVcopy(&v[0], spos);
	rcVcopy(&v[3], epos);
	m_offMeshConCount++;
}

void InputGeom::deleteOffMeshConnection(int i)
{
	m_offMeshConCount--;
	fixed64* src = &m_offMeshConVerts[m_offMeshConCount*3*2];
	fixed64* dst = &m_offMeshConVerts[i*3*2];
	rcVcopy(&dst[0], &src[0]);
	rcVcopy(&dst[3], &src[3]);
	m_offMeshConRads[i] = m_offMeshConRads[m_offMeshConCount];
	m_offMeshConDirs[i] = m_offMeshConDirs[m_offMeshConCount];
	m_offMeshConAreas[i] = m_offMeshConAreas[m_offMeshConCount];
	m_offMeshConFlags[i] = m_offMeshConFlags[m_offMeshConCount];
}

void InputGeom::addConvexVolume(const fixed64* verts, const int nverts,
								const fixed64 minh, const fixed64 maxh, unsigned char area)
{
	if (m_volumeCount >= MAX_VOLUMES) return;
	ConvexVolume* vol = &m_volumes[m_volumeCount++];
	memset(vol, 0, sizeof(ConvexVolume));
	memcpy(vol->verts, verts, sizeof(fixed64)*3*nverts);
	vol->hmin = minh;
	vol->hmax = maxh;
	vol->nverts = nverts;
	vol->area = area;
}

void InputGeom::deleteConvexVolume(int i)
{
	m_volumeCount--;
	m_volumes[i] = m_volumes[m_volumeCount];
}